#define MAX(X,Y) ((X) > (Y) ? : (X) : (Y))
#define MIN(X,Y) ((X) < (Y) ? : (X) : (Y))
//#include "tchar.h"
#include<iostream>
#include <iomanip>
//#include<sstream>
#include<string>
#include<fstream>
#include<math.h>
#include <cassert>
#include <time.h>

#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <mpi.h>


static gsl_rng * rg = gsl_rng_alloc(gsl_rng_mt19937);
int   mpi_rank;
int    mpi_ranks;



//#include<gsl/gsl_randist.h>
//#include "flysim.h"

using namespace std;

ofstream ofile("SMCout1.txt",ios::out);
const int ciNoOfParameters=6;
//float dParameterLowerBounds[ciNoOfParameters];
//float dParameterUpperBounds[ciNoOfParameters];
const int ciDimensionOfData=ciNoOfParameters;
const int ciNoOfAcceptancesPerGeneration=500;//1000;
//int iNumberOfGenerationsForSMCAnalysis=20;
float ThisGensParams[ciNoOfAcceptancesPerGeneration][ciNoOfParameters];
int TempArray[ciNoOfParameters];
bool bDoPureRejection=0; // generally this will be set to 0, but if we set it to one it will do rejection without SMC ( y setting the number of generations equal to 1
//
//
//
//// Some useful function declarations follow
//
long int ranseed= -153425;

////*********************************************
///*  Random number generator from Numerical recipes (Ed 2, ran1) */
#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
float rand01(/*long *idum*/)
{
/*	int j;
	long k;
	static long iy=0;
	static long iv[NTAB];
	float temp;
	if(*idum<=0 || !iy) {
		if(-(*idum) < 1) *idum=1;
        else *idum= -(*idum);
		for(j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ;
			*idum=IA*(*idum-k*IQ) -IR*k;
			if(*idum < 0) *idum += IM;
			if(j <NTAB) iv[j]= *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ;
	*idum=IA*(*idum-k*IQ)-IR*k;
	if(*idum < 0) *idum += IM;
	j=iy/NDIV;
	iy=iv[j];
	iv[j]= *idum;
	if((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
*/
return (float)rand()/RAND_MAX;

}
#ifdef TRAND
main() {
	int i,bin[10];
	long dummy= -9284322;
	for(i=0;i<10;i++) bin[i]=0;
	for(i=0;i<20000;i++)
		bin[(int)10*rand01(&dummy)]++;
	for(i=0;i<10;i++)
		printf("\n\t%d %lf",i,bin[i]/20000.0);
}
#endif
////**********************************************
//

#define SQRT_2 sqrt(2.0)
#define SQRT_PI sqrt(3.14159265)

/*Generator of normally distributed random variable from Numerical recipes (Ed 2, gasdev) */
/*float gasdev(long *idum)
{
	static int iset=0;
	static float gset;
	float fac,rsq,v1,v2;
	if (iset==0) {
		do{
			v1=2.0*gsl_rng_uniform_pos(rg)-1.0;
			v2=2.0*gsl_rng_uniform_pos(rg)-1.0;
			rsq=v1*v1+v2*v2;
		}while(rsq>=1.0 ||rsq==0.0);
		fac=sqrt(-2.0*log(rsq)/rsq);
		gset=v1*fac;
		iset=1;
		return v2*fac;
	}else{
		iset=0;
		return gset;
	}
}*/

float NormalRV(float Mean, float StdDev)
{

    //return Mean+StdDev*gasdev(&ranseed);
	return Mean+gsl_ran_gaussian(rg,StdDev);
}

//*******************************************************
float erfcc(float x)
{
	float t,z,ans;

	z=fabs(x);
	t=1.0/(1.0+0.5*z);
	ans=t*exp(-z*z-1.26551223+t*(1.00002368+t*(0.37409196+t*(0.09678418+
                                                             t*(-0.18628806+t*(0.27886807+t*(-1.13520398+t*(1.48851587+
                                                                                                            t*(-0.82215223+t*0.17087277)))))))));
	return x >= 0.0 ? ans : 2.0-ans;
}

//********************************************************

float P_Normal_greater_than_x(float xx, float mean, float sigma)
{
    // returns normalising constants for normal random variables
	xx=xx-mean;
	xx=xx/sigma;
	return erfcc(xx/SQRT_2)/2.0;
}

//********************************************************

float NormalDensity(float x, float mean, float StdDev)
{
	float dDen;
	dDen=1.0/(StdDev*SQRT_2*SQRT_PI);
	dDen=dDen*exp(-1*(x-mean)*(x-mean)/(2*StdDev*StdDev));
	return dDen;
}

////*******************************************************
//
float ExponentialRV(float dLambda) // generates exponential random variables
{
	float p,x;
	p=gsl_rng_uniform_pos(rg);
	x=-1*log(p)/dLambda;
	return x;
}
//
////*******************************************************
////*******************************************************
////********class implemetations follow*******************
////*******************************************************
////*******************************************************
//
//

class SMC{
public:
    double TestParamValues[ciNoOfParameters];
    double StdDev;
    const static int ciNoOfTestDatasets=10;
    double dRejectionThreshold;
    int iMaxNumberOfAttempts;
    int iNumberOfGenerationsForSMCAnalysis;
    int ciNoOfAcceptsNeeded;
    double TestData[ciNoOfTestDatasets][ciDimensionOfData];
    
    double SampledParameters[ciNoOfParameters];
    double GeneratedData[ciDimensionOfData];
    double NewAcceptedParameterValues[ciNoOfAcceptancesPerGeneration][ciNoOfParameters];
    double OldAcceptedParameterValues[ciNoOfAcceptancesPerGeneration][ciNoOfParameters];
    double dWeightsOfOldParametersValues[ciNoOfAcceptancesPerGeneration];
    double dWeightsOfNewParametersValues[ciNoOfAcceptancesPerGeneration];
    float dParameterLowerBounds[ciNoOfParameters];
    float dParameterUpperBounds[ciNoOfParameters];

    
    SMC();
    ~SMC();
    void PrintParameterValues(bool);
    void PrintWeightedParameterValues();
    void BuildTestData();
    void PrintTestData();
    void RunSMCAnalysis();
    void DoThisGenerationsRejection(int);
    void GenerateParameters(int);
    void GenerateData();
    float MeasureDistanceBetweenDataAndParameters();
    void UpdateParameterValuesAndWeights();
    void InitializeArrays();
    void NormalizeNewParameterWeights();
    void CalculateParameterWeights(int);
    float SampleFromParameterPrior(int);
    
};

SMC::~SMC(){
}

SMC::SMC(){
    iNumberOfGenerationsForSMCAnalysis=100; // set this high for now..
    iMaxNumberOfAttempts=160000000;
    dRejectionThreshold=500;//12;
    StdDev=0.10;;//5;//0.5;
#define BOUND 5
if (mpi_rank==0)
cerr<<"True parameter values are: ";
for (uint i=0;i<ciDimensionOfData;i++) {
	if (mpi_rank==0) {
	TestParamValues[i]=gsl_ran_flat(rg,0,10);
	cerr<<"\t"<<TestParamValues[i];	
	dParameterLowerBounds[i]=TestParamValues[i]-BOUND;
	dParameterUpperBounds[i]=TestParamValues[i]+BOUND;
	}
	MPI::COMM_WORLD.Bcast(&TestParamValues[i],1,MPI::DOUBLE,0);
	MPI::COMM_WORLD.Bcast(&dParameterLowerBounds[i],1,MPI::FLOAT,0);
	MPI::COMM_WORLD.Bcast(&dParameterUpperBounds[i],1,MPI::FLOAT,0);
}
cerr<<endl;
    ciNoOfAcceptsNeeded=ciNoOfAcceptancesPerGeneration;
}

void SMC::PrintWeightedParameterValues() {
float weighted_parameters[ciNoOfParameters];
for (int i=0;i<ciNoOfParameters;i++)  {
	weighted_parameters[i]=0;
	for (int j=0;j<ciNoOfAcceptancesPerGeneration;j++) 
		weighted_parameters[i]+=NewAcceptedParameterValues[j][i]*dWeightsOfNewParametersValues[j];
	if (i==0)
		 fprintf(stderr,"%f",weighted_parameters[i]);
	else
		fprintf(stderr,"\t%f",weighted_parameters[i]);
}
fprintf(stderr,"\td=%f\n",MeasureDistanceBetweenDataAndParameters());
if (MeasureDistanceBetweenDataAndParameters()<0.20) {
	fprintf(stderr,"Got below our threshold!");
	exit(EXIT_SUCCESS);

}

}

void SMC::PrintParameterValues(bool bPrintToFile)
{
    if (bPrintToFile==0)
    {
        std::cout<<"\n*************Parameter values follow***********";
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            std::cout<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                std::cout<<NewAcceptedParameterValues[i][j]<<" ";
            }
            std::cout<<"     weight: "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
        std::cout<<"\n***********************************************";
    }
    else
    {
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            ofile<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                ofile<<NewAcceptedParameterValues[i][j]<<" ";
            }
            ofile<<"      "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
    }
}


void SMC::BuildTestData()
{
    for (int i=0; i<ciNoOfTestDatasets; i++) 
   	for (int j=0;j<ciNoOfParameters;j++) { 
		TestData[i][j]=NormalRV(TestParamValues[j], StdDev); 
	}
}

void SMC::PrintTestData()
{

float *summary=(float*)calloc(ciDimensionOfData,sizeof(float));
    std::cerr<<"*************Test data follows***********";
    for ( int i = 0; i < ciNoOfTestDatasets; ++i ) {
        std::cerr<<endl;
        for ( int j=0; j< ciDimensionOfData; j++) {
		summary[j]+=TestData[i][j];
            std::cerr<<TestData[i][j]<<"  ";
        }
    }
    std::cerr<<"\n*****************************************\n";
        for ( int j=0; j< ciDimensionOfData; j++) {
                cerr<<summary[j]/ciNoOfTestDatasets<<" ";
        }

    std::cerr<<"\n*****************************************\n";
    
}

void SMC::RunSMCAnalysis()
{
  
	double reject_threshold[mpi_ranks];
 
//if (mpi_rank==0) { 
	if (mpi_rank==0)
    BuildTestData();

	MPI::COMM_WORLD.Bcast(&TestData,ciNoOfTestDatasets*ciNoOfParameters,MPI::DOUBLE,0);

    PrintTestData();
//}
    if (bDoPureRejection==1)
        iNumberOfGenerationsForSMCAnalysis=1;
    for (int i=0;i<iNumberOfGenerationsForSMCAnalysis;i++)
    {
        DoThisGenerationsRejection(i);
        UpdateParameterValuesAndWeights();
	if (mpi_rank==0) {
		PrintParameterValues(0);
		PrintWeightedParameterValues();
	}

	/*
	reject_threshold[mpi_rank]=MeasureDistanceBetweenDataAndParameters();

	for (int j=0;j<mpi_ranks;j++)
		MPI::COMM_WORLD.Bcast(&reject_threshold[j],1,MPI::DOUBLE,j);

	dRejectionThreshold=0;//MeasureDistanceBetweenDataAndParameters();
	for (int j=0;j<mpi_ranks;j++)
		dRejectionThreshold+=reject_threshold[j]/mpi_ranks;
	*/
        dRejectionThreshold=MeasureDistanceBetweenDataAndParameters();
    }
if (mpi_rank==0) {
    PrintParameterValues(0);
    PrintParameterValues(1);
}
}



void SMC::GenerateParameters(int iGen)
{
    double p,dProbSoFar;
    int i,j;
    if (iGen==0)
    {
        //SampledParameters[0]=SampleFromParameterPrior(0);
        //SampledParameters[1]=SampleFromParameterPrior(1);
        //SampledParameters[2]=SampleFromParameterPrior(2);

	for (i=0;i<ciNoOfParameters;i++)
		SampledParameters[i]=SampleFromParameterPrior(i);
    }
    else
    {
        for (i=0;i<ciNoOfParameters;i++)
        {

	    p=gsl_rng_uniform_pos(rg);


            dProbSoFar=0;
            j=-1;

            	while (dProbSoFar<p)
           	 {
           	     j++;
               		 dProbSoFar+=dWeightsOfOldParametersValues[j];
            	}

	/*if (j>=ciNoOfAcceptsNeeded)
	{
		fprintf(stderr,"j=%d, ciNoOfAcceptsNeeded=%d, p=%f\n",j,ciNoOfAcceptsNeeded,p);
	

	}*/
            assert(j<=ciNoOfAcceptsNeeded);
            SampledParameters[i]=OldAcceptedParameterValues[j][i];
            // add noise
            SampledParameters[i]+=NormalRV(0, StdDev);
        }
    }
}

void SMC::GenerateData()
{

	for (uint i=0;i<ciNoOfParameters;i++)
 		GeneratedData[i]=NormalRV(SampledParameters[i],StdDev);
//    GeneratedData[1]=NormalRV(SampledParameters[1],StdDev);
  //  GeneratedData[2]=NormalRV(SampledParameters[2],StdDev);
}

float SMC::MeasureDistanceBetweenDataAndParameters()
{
    float d=0;
    int i,j;
    for (j=0;j<ciNoOfTestDatasets;j++)
        for (i=0; i<ciDimensionOfData;i++)
            d+=(GeneratedData[i]-TestData[j][i])*(GeneratedData[i]-TestData[j][i]);
    return d;
}

void SMC::DoThisGenerationsRejection(int iGen) //FIXME
{
    int iAccepts=0,iAttempts=0,j,NoOfAcceptsNeededPerRank=ciNoOfAcceptancesPerGeneration/mpi_ranks;
    float dDist;
double mpi_NewAcceptedParameterValues[mpi_ranks][NoOfAcceptsNeededPerRank][ciNoOfParameters];

for (iAttempts=0;iAccepts<NoOfAcceptsNeededPerRank;iAttempts++) {
	GenerateParameters(iGen);
	GenerateData();
	if ((dDist=MeasureDistanceBetweenDataAndParameters())<dRejectionThreshold) {
		for (j=0;j<ciNoOfParameters;j++) 
			mpi_NewAcceptedParameterValues[mpi_rank][iAccepts][j]=SampledParameters[j];
			//NewAcceptedParameterValues[iAccepts+mpi_rank*NoOfAcceptsNeededPerRank][j]=SampledParameters[j];
		iAccepts++;
	}
}

MPI::COMM_WORLD.Bcast(&dWeightsOfNewParametersValues,ciNoOfAcceptsNeeded,MPI::DOUBLE,0);

for (int i=0;i<mpi_ranks;i++)
	MPI::COMM_WORLD.Bcast(&mpi_NewAcceptedParameterValues[i],NoOfAcceptsNeededPerRank*ciNoOfParameters,MPI::DOUBLE,i);

for (int i=0;i<ciNoOfAcceptsNeeded;i++)
	for (j=0;j<ciNoOfParameters;j++)
		NewAcceptedParameterValues[i][j]=mpi_NewAcceptedParameterValues[i/NoOfAcceptsNeededPerRank][i%NoOfAcceptsNeededPerRank][j];

    CalculateParameterWeights(iGen);
    std::cout<<"\nGeneration "<<iGen<<" required "<<iAttempts<<" iterations";
}

void SMC::UpdateParameterValuesAndWeights()
{
    int i,j;
    for (i=0;i<ciNoOfAcceptsNeeded;i++)
    {
        dWeightsOfOldParametersValues[i]=dWeightsOfNewParametersValues[i];
        for (j=0;j<ciNoOfParameters;j++)
            OldAcceptedParameterValues[i][j]=NewAcceptedParameterValues[i][j];
    }
}

void SMC::CalculateParameterWeights(int iGen)
{
    int i,j,k;
    float dDensity,dDen,dTotalDensity,dSumOfWeights=0;
    // for each set of New Parameter values
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
    {
        if (iGen==0)
        {
            dWeightsOfNewParametersValues[k]=1.0/ciNoOfAcceptsNeeded;
            dSumOfWeights=1;
        }
        else
        {
            dTotalDensity=1;
            // for each parameter
            for (i=0;i<ciNoOfParameters;i++)
            {
                dDensity=0;
                // calculate the density of generating this value - need to go through each old parameter value to to this
                for (j=0;j<ciNoOfAcceptsNeeded;j++)
                {
                    //dDen=gsl_ran_gaussian_pdf(NewAcceptedParameterValues[k][i]-OldAcceptedParameterValues[j][i],StdDev);
                    dDen=NormalDensity(NewAcceptedParameterValues[k][i], OldAcceptedParameterValues[j][i], StdDev);
                    dDensity+=(dDen/ciNoOfAcceptsNeeded);
                }
                dTotalDensity=dTotalDensity*dDensity;
            }
            dWeightsOfNewParametersValues[k]=dTotalDensity;
        }
    }
    
    // They now need to be inverted to recognize the importance sampling correction
    if (iGen>0)
        for (k=0;k<ciNoOfAcceptsNeeded;k++)
        {
            dWeightsOfNewParametersValues[k]=1.0/dWeightsOfNewParametersValues[k];
            dSumOfWeights+=dWeightsOfNewParametersValues[k];
        }
    
    
    // Now normalize them go become probs ready for sampling in the next generation
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
        dWeightsOfNewParametersValues[k]=dWeightsOfNewParametersValues[k]/dSumOfWeights;
}

void SMC::InitializeArrays()
{
    for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
        dWeightsOfNewParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        dWeightsOfOldParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        for ( int j=0; j< ciNoOfParameters; j++) {
            OldAcceptedParameterValues[i][j] = -9;
            NewAcceptedParameterValues[i][j]=-9;
        }
    }
}

float SMC::SampleFromParameterPrior(int iWhichParameter)
{
    // we assume a uniform prior for now
    float dVal;
    dVal=gsl_rng_uniform_pos(rg)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    //dVal=rand01(&ranseed)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    return dVal;
}


int main(int argc, const char * argv[])
{
MPI::Init();
mpi_rank=MPI::COMM_WORLD.Get_rank();
mpi_ranks=MPI::COMM_WORLD.Get_size();

assert(ciNoOfAcceptancesPerGeneration%mpi_ranks==0 );

gsl_rng_set(rg,time(0)+mpi_rank);

    // insert code here...
    SMC MyTester;
    MyTester.InitializeArrays(); //have this
	if (mpi_rank==0)
    MyTester.PrintParameterValues(0); // have this in integrated...
    MyTester.RunSMCAnalysis(); //have this in integrated...
    // MyTester.PrintParameterValues(0);
    //MyTester.PrintParameterValues(1);
 MPI::Finalize();
    return 0;
}
